;;http://softsurfer.com/Archive/algorithm_0104/algorithm_0104.htm

(defun v-op (op a b)
	(map 'vector op a b))
	
(defun l-op (op a b)
	(map 'list op a b))

(defun nl-op (op n a)
	(map 'list (lambda (x) (funcall op x n)) a))
	
(defun line (s point vector)
	(l-op '+ point (nl-op '* s vector)))

(defun l-dot (a b)
	(apply '+ (l-op '* a b)))

(defun l-vector-length (v)
	(sqrt (apply '+ (mapcar (lambda (n) (* n n)) v))))
	
(defun l-vector-to-vector-projection (v u)
	(let ((l (l-vector-length u)))
		(nl-op '* (/ (l-dot v u) (* l l)) u)))
	
(defun l-vector-to-unit-vector-projection (v u)
	(nl-op '* (l-dot v u) u))

(defun 3d-line-plane-intersection (plane-point plane-normal-vector line-point line-vector)
	(line (/ (l-dot plane-normal-vector (l-op '- plane-point line-point))
			 (l-dot plane-normal-vector line-vector))
					line-point line-vector))
				
(defun l-matrix-vector-mult (m v)
	(mapcar (lambda (x) (l-dot x v)) m))
	
(defun l-matrix-mult (m n)
	(mapcar (car m) ))
	
(defun l-vector-rotation (deg vector rotation-matrix front left top)
	(let ((m '(front left top)))
		(matrix-vector-mult (matrix-mult m rotation-matrix) vector)))

(defun l-yaw-rotation (deg vector front left top)
	(let ((r '(((cos deg)     (sin deg) 0)
			   ((- (sin deg)) (cos deg) 0)
			   (0              0        1))))
		(l-vector-rotation deg vector r front left top)))
						
(defun l-pitch-rotation (deg vector front left top) 
	(let ((r '(((cos deg) 0 (- (sin deg)))
			   (0         1           0)
			   ((sin deg) 0    (cos deg)))))
		(l-vector-rotation deg vector r front left top)))
								
(defun l-roll-rotation (deg vector front left top) 
	(let ((r '((1     0           0)
			   (0 (cos deg) (- (sin deg)))
			   (0 (sin deg)    (cos deg)))))
		(l-vector-rotation deg vector r front left top)))
								
								
								
								
								
								
								